
class PIDController {
 public:
  PIDController();
  ~PIDController();

  void setKp(double Kp) { _Kp = Kp; }
  void setKi(double Ki) { _Ki = Ki; }
  void setKd(double Kd) { _Kd = Kd; }
  void setAntiWindup(double min, double max) { _minITerm = min; _maxITerm = max; }
  
  double compute(double error, double timeStep);

 private:
  double  _Kp;
  double  _Ki;
  double  _Kd;
  
  double _iTerm;
  double _minITerm;
  double _maxITerm;
  double _lastState;
};

inline PIDController::PIDController() {
  _Kp = 0.0;
  _Ki = 0.0;
  _Kd = 0.0;
  _iTerm = 0.0;
  _lastState = 0.0;       // Todo set to state on first tick?
  _minITerm = -1e23;      // Essentially Negative Infinity
  _maxITerm = 1e23;       // Essentially Positive Infinity
}

// BASED ON http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-reset-windup/
inline double PIDController::compute(double state, double setpoint) {
  
  double error = setpoint - state;
  
  double pTerm = _Kp*error;
  _iTerm += _Ki*error;
  double dTerm = _Kd*(state-_lastState);
  
  // Apply anti-windup:
  if(_iTerm > _maxITerm) _iTerm= _maxITerm;
  else if(_iTerm < _minITerm) _iTerm= _minITerm;
  
  double output = pTerm + _iTerm - dTerm;

  std::cout << "P:" << pTerm << "  I:" << _iTerm << "  D:" << dTerm << "\n";
  
  _lastState = state;
  return output;
}
